/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/helpSrc/guide_help.cpp
 * @Description  : 用于AGV导航的GuideHelper类的实现，包含初始化、反馈过滤、航姿推算、姿态信息收集以及角速度控制数据生成等功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:50:17
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "guide_help.h" 
#include "public_kernel.h"
const int MAX_WAIT_CYCLES = 2;    // 9 The number of cycles for each scanner data (450ms)
const int SCANNER_FBOK_GATE = 3;  // 5 continous OK segments will be regarded as "real OK"
const int SCANNER_NO_FB_GATE = 5; // 10 continous "no feedback" cycles is regarded as "get lost"

const float X_LIMIT = 0.2f;       // 20cm  change by qf
const float THITA_LIMIT = 0.35f;  // 20 degree

ModifyCtrl GuideHelper::modify_ctrl_ = ModifyCtrl(); // 纠偏量
//////////////////////////////////////////////////////////////////////
// implementation of the GuideHelper class
GuideHelper::GuideHelper() 
{
    InitValue();
}

/**
 * @brief 初始化 GuideHelper 类的成员变量
 *
 * 该函数用于初始化 GuideHelper 类的成员变量，将它们设置为默认值。
 */
void GuideHelper::InitValue()
{
    trajectory_ = nullptr;
    linear_velocity_ = 0.0f;
    angular_velocity_ = 0.0f;
    dead_reckoning_distance_ = 0.0f;
    dead_reckoning_limit_ = 0.0f;
    quality_ = 0.0f;
    signal_triggered_ = false;
    location_feedback_ = LocationFeedback::LOC_DEAD_RECKONING;
}

/**
 * @brief 创建GuideHelper实例
 *
 * 该函数用于创建GuideHelper实例，并初始化相关成员变量。
 *
 * @param check_mark 是否进行检查标记
 */
void GuideHelper::Create(bool check_mark) 
{
    check_mark_ = check_mark;
    mark_finished_ = false;
    normal_count_ = SCANNER_FBOK_GATE;
    failed_count_ = 0;
}

/**
 * @brief 设置轨迹和剩余区域大小
 *
 * 该函数用于设置轨迹对象和剩余区域大小。
 *
 * @param trajectory 轨迹对象指针
 * @param area       剩余区域大小
 */
void GuideHelper::SetTraj(Trajectory* trajectory, float area)
{
    trajectory_ = trajectory;
    area_remain_ = area;
}

/**
 * @brief 开始函数
 *
 * @param dead_reckoning_limit 推算限制值
 */
void GuideHelper::OnStart(float dead_reckoning_limit)
{
    dead_reckoning_limit_ = dead_reckoning_limit;
}

/**
 * @brief 过滤反馈结果
 *
 * 根据给定的条件对反馈结果进行过滤。
 *
 * @param hold_current_feedback 是否保持当前的反馈结果
 * @param current_feedback 当前反馈结果，通过引用传递，函数内部会修改其值
 */
void GuideHelper::FilterFeedBack(bool hold_current_feedback, bool& current_feedback)
{
    if (hold_current_feedback) {
        // Normal response ok
        normal_count_ = std::min(normal_count_ + 1, SCANNER_FBOK_GATE);
        failed_count_ = 0;
    } else {
        // No response count
        failed_count_++;
        if (failed_count_ >= SCANNER_FBOK_GATE) {
            failed_count_ = SCANNER_FBOK_GATE;
            normal_count_ = 0;
        }
    }

    if (normal_count_ < SCANNER_FBOK_GATE) {
        current_feedback = false;
    }
}

/**
 * @brief 航姿推算
 *
 * 更新位置反馈状态和等待周期，并计算航姿推算距离。
 * the navigation loss event will be reported after autonomous walking for 0.3m
 * @param current_feedback 当前反馈状态，true表示反馈正常，false表示反馈异常
 */
void GuideHelper::TryToDeadReckonRun(bool& current_feedback) 
{
    if (current_feedback) {
        // Normal Regarded as "Feedback OK"
        location_feedback_ = LocationFeedback::LOC_FEEDBACK_OK;
        wait_cycle_ = 0;
        dead_reckoning_distance_ = 0.0f;
    } else if (wait_cycle_++ < MAX_WAIT_CYCLES) {
        // Waiting Regarded as "Feedback OK"
        location_feedback_ = LocationFeedback::LOC_FEEDBACK_WAITING;
        dead_reckoning_distance_ = 0.0f;
        current_feedback = true; // force change is ok
    } else if (dead_reckoning_distance_ < dead_reckoning_limit_) {
        // When spinning, the linear velocity is 0, and the distance calculation here is unreasonable
        location_feedback_ = LocationFeedback::LOC_DEAD_RECKONING;
        wait_cycle_ = MAX_WAIT_CYCLES;
        dead_reckoning_distance_ += fabs(linear_velocity_) * GetCtrlCycle();
        current_feedback = true;
    } else {
        // Failed
        location_feedback_ = LocationFeedback::LOC_FEEDBACK_FAIL;
        wait_cycle_ = MAX_WAIT_CYCLES;
        current_feedback = false;
    }
}

/**
 * @brief 收集姿态信息
 *
 * 该函数用于收集设备的姿态信息，并根据需要决定是否仅收集信息或进行进一步处理。
 * Generate:project data: to devX devT progress
 * @param posture 姿态信息结构体引用，用于存储收集到的姿态信息。
 * @param only_collect_info 是否仅收集信息而不进行进一步处理。
 */
void GuideHelper::CollectInfo(Posture& posture, bool only_collect_info) 
{
    DEBUG_OUT("GuideHelper: CollectInfo: get pose start");

    float x = 0.0f, y = 0.0f, theta = 0.0f;
    quality_ = -200.0f;

    bool current_feedback = GetLaserAutoPos(x, y, theta, quality_, false, true) && quality_ > 5.0f;
    bool quality_ok = quality_ > 5.0f;
    // 当报脱轨时，使用此标识报定位数据超时
    bool hold_current_feedback = current_feedback;

    bool over_limit_x = false;
    bool over_limit_theta = false;
    bool over_limit_progress = false;
    bool posture_estimate_ok = true;
    // 当报脱轨时，鉴定是否是由于误差超限/还是数据超时
    // 位姿是否评估成功(是否在路线附近)
    if (only_collect_info) {
        // 无导航时，设置反馈模式为盲走模式
        location_feedback_ = LocationFeedback::LOC_DEAD_RECKONING;
        if (current_feedback) {
            posture = Posture(x, y, theta);
        }
        return;
    }

    if (current_feedback) {
        posture = Posture(x, y, theta);
        posture_estimate_ok = trajectory_->ProjectPosture(posture, &project_data_);
        
        //当运行超过节点一定距离内，仍认为距离有效，可以正常判断有效
        bool out_end = project_data_.type_ == ProjectData::ProjectionType::type_outside_end;
        if (!posture_estimate_ok && out_end) {
            float range = trajectory_->GetRange();
            bool valid = fabs(fabs(project_data_.y_) - fabs(range)) <= 0.35f;
            if (valid) {
                posture_estimate_ok = true;
                project_data_.relative_progress_ = fabs(project_data_.y_);
                DEBUG_OUT("outside end: Estimate: force set posture_estimate_ok; realProg=" << project_data_.y_);
            } else {
                DEBUG_ERROR_OUT("outside end: posture estimate invalid, error distance too long; realProg=" << project_data_.y_ << ", range=" << range);
            }
        }

        current_feedback = posture_estimate_ok;

        over_limit_x = fabs(project_data_.x_) > X_LIMIT;
        over_limit_theta = abs(Angle(project_data_.theta_)) > THITA_LIMIT;
        float progress_diff = fabs(trajectory_->GetRange() - area_remain_ - project_data_.relative_progress_);
        over_limit_progress = progress_diff > 0.5f;

        if (over_limit_x || over_limit_theta || over_limit_progress) {
            current_feedback = false;
            DEBUG_OUT("error: laser data over limit:"
                      << " over_limit_x=" << over_limit_x
                      << ", over_limit_theta=" << over_limit_theta
                      << ", over_limit_progress=" << over_limit_progress
                      << ", x(dev)=" << project_data_.x_
                      << ", y(prog)=" << project_data_.y_
                      << ", theta=" << Angle(project_data_.theta_).radian_
                      << ", progress_diff=" << progress_diff << ", posture_estimate_ok=" << posture_estimate_ok);
        } else {
            // fx 符号：行进方向的左负右正(参考轨迹)
            DEBUG_OUT("normal: laser dev: x(dev)=" << project_data_.x_ << ", y(prog)=" << project_data_.y_
                                                   << ", theta=" << Angle(project_data_.theta_).NormAngle2()
                                                   << ", progress_diff=" << progress_diff
                                                   << ", posture_estimate_ok=" << posture_estimate_ok);
        }
    }

    FilterFeedBack(hold_current_feedback, current_feedback);
    TryToDeadReckonRun(current_feedback);

    if (!current_feedback) {
        std::string msg;
        msg += over_limit_x ? "deviation y is too big; " : "";
        msg += over_limit_theta ? "deviation angle is too big; " : "";
        msg += over_limit_progress ? "deviation x progress is too big; " : "";
        msg += !posture_estimate_ok ? "location data is not near the path; " : "";
        msg += !quality_ok ? "confidence level is too low; " : "";
        // 如果不是位姿偏差过大或评估位姿失败，且数据不能使用，则是定时超时问题
        bool loc_timeout = !over_limit_x && !over_limit_theta && !over_limit_progress && posture_estimate_ok && quality_ok;
        msg += loc_timeout ? "location data timeout (>150ms);" : "";

        TryToOccurPathLost(msg, true);
    }
}

/**
 * @brief 获取等级参数
 *
 * 根据轨迹类型和方向，找到对应的参数值。
 *
 * @param a1 第一个参数值，通过引用传递
 * @param a2 第二个参数值，通过引用传递
 */
void GuideHelper::FindLevelParam(float& a1, float& a2) {
    if (!trajectory_) {
        DEBUG_OUT("find trajectory is null");
        return;
    }

    SHORT traj_type = trajectory_->GetType();
    bool side = static_cast<int>(TrajType::SIDE_TRAJ) == traj_type;
    bool curve = static_cast<int>(TrajType::LINE_TRAJ) != traj_type && static_cast<int>(TrajType::SIDE_TRAJ) != traj_type;
    bool forward = linear_velocity_ > 0.000001f;

    ParamSubType dir_param = curve ? ParamSubType::LaserCurve :
                                    (side ? ParamSubType::LaserSide :
                                    (forward ? ParamSubType::LaserLineForward : ParamSubType::LaserLineBack));

    for (int i = 0; i < 10; ++i) {
        bool find_next_index = curve ? fabs(angular_velocity_) > 0.1 * (i + 1) : fabs(linear_velocity_) > MotionParm::NormalVel(i);
        if (find_next_index) {
            if (i == 9) {
                a1 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_y_;
                a2 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_angle_;
                break;
            }
            continue;
        } else {
            a1 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_y_;
            a2 = MotionParm::revise_params_.laser_params_[static_cast<int>(dir_param)][i].modify_angle_;
            break;
        }
    }
}

/**
 * @brief 生成角速度控制数据
 *
 * 根据提供的姿态信息和反馈状态生成角速度控制数据。
 * 产生导航纠偏量，AMR只产生角速度;全向车产生vx vy w三个反向的矢量
 *   新改导航模式为（非无导航时)在盲走阶段时(30cm内)，也计算导航纠偏； 2023-0830
 *   注意：无导航模式 不应该调用此函数
 * @param posture 姿态信息
 * @param feedback_ok 反馈状态标志
 * @param ignore_y_error 是否忽略Y轴误差
 *
 * @return 生成的角速度控制数据
 */
ModifyCtrl GuideHelper::GenAngularVel(const Posture& posture, bool& feedback_ok, bool ignore_y_error)
{
    feedback_ok = true;
    switch (location_feedback_) {
        case LocationFeedback::LOC_FEEDBACK_WAITING:
        case LocationFeedback::LOC_FEEDBACK_OK:
        case LocationFeedback::LOC_DEAD_RECKONING:
        {
            float a1 = 0.0f, a2 = 0.0f;
            // #1 仅仅使用来更新 m_ProjectData 数据;纠偏计算不用此量
            // 具体纠偏计算：FollowPath 内部 重新计算
            trajectory_->ProjectPosture(const_cast<Posture&>(posture), &project_data_);

            // #2 find revise param1 param2
            FindLevelParam(a1, a2);

            // #3 followPath to output ctrlVel
            // #3-1 当前的控制周期(50ms)
            float period = GetCtrlCycle();

            // #3-2 角速度纠偏加速度：低速时加速度8°/s^2 ;高速加速度 6°/s^2(旧:20°/s^2 较大)
            // 角速度斜坡值(旧:方盛现场 amr)
            // 旧:AMR:const float fAngleVelStep = fabs(m_fLinearVel) > 0.5 ? period * TO_RADIAN(20) : period * TO_RADIAN(20);
            // 新:弱化纠偏角速度斜坡
            // const float fAngleVelStep = fabs(m_fLinearVel) > 0.5 ? period * TO_RADIAN(6) : period * TO_RADIAN(8);
            // 增加梯度角加速度值
            const float angle_vel_step = period * TO_RADIAN(GetAngleVelStep(linear_velocity_));

            // #3-3 vx vy 方向纠偏加速度： 低速时加速度0.15m/s^2 ;高速加速度 0.1m/s^2
            // 速度斜坡值
            const float vel_step = fabs(linear_velocity_) > 0.5f ? period * 0.1f : period * 0.15f;

            float current_linear_vel = linear_velocity_;
            ModifyCtrl ctrl_data = FollowPath(&posture, trajectory_, current_linear_vel, a1, a2, ignore_y_error);

           if (ctrl_data.CanIgnore()) {
                modify_ctrl_.Clear();
                DEBUG_OUT("FollowPath can ignore; force clear modify_ctrl_");
            }
            Approach(modify_ctrl_.delta_velocity_x, ctrl_data.delta_velocity_x, vel_step);
            Approach(modify_ctrl_.delta_velocity_y, ctrl_data.delta_velocity_y, vel_step);
            Approach(modify_ctrl_.angular_velocity, ctrl_data.angular_velocity, angle_vel_step);
        }
        break;
        // case LOC_DEAD_RECKONING:
        // {
        //     bFeedbackOK = FALSE;
        //     m_modifyCtrl.Clear();
        // }
        // break;
        case LocationFeedback::LOC_FEEDBACK_FAIL:
        {
            // NoResponse Mode
            feedback_ok = false;
            modify_ctrl_.Clear();
        }
        break;
    }

    return modify_ctrl_;
}

/**
 * @brief 获取测量相对进度
 *
 * 获取当前测量任务的相对进度，并判断当前位置反馈状态是否正常。
 *
 * @param relative_progress 用于存储相对进度的引用变量
 * @return 如果位置反馈状态正常，则返回true；否则返回false
 */
bool GuideHelper::GetMeasuredRelProgress(float& relative_progress) {
    bool ret = location_feedback_ == LocationFeedback::LOC_FEEDBACK_OK;
    relative_progress = project_data_.relative_progress_;
    return ret;
}

/**
 * @brief 根据速度获取角度速度步长
 *
 * 根据给定的速度值，返回对应的角度速度步长。
 *
 * @param velocity 给定的速度值
 * @return 返回对应的速度步长
 */
float GuideHelper::GetAngleVelStep(const float& velocity) const
{
    float step = 0.01f;
    float v = fabs(velocity);
    float acc1 = MotionParm::correct_params_.low_ang_acc_;
    float acc2 = MotionParm::correct_params_.mid_ang_acc_;
    float acc3 = MotionParm::correct_params_.high_ang_acc_;
    float acc4 = MotionParm::correct_params_.super_high_ang_acc_;

    if (v <= 0.5f) {
        step = acc1;
    } else if (v <= 1.0f) {
        step = acc2;
    } else if (v <= 1.5f) {
        step = acc3;
    } else if (v <= 2.0f) {
        step = acc4;
    } else {
        step = acc4;
    }

    return step;
}
